#include <QDebug>

#include <stdio.h>
#include <stdlib.h>
#include <termios.h>		//serial port
#include <unistd.h>
#include <signal.h>

#include "RS232Motor.h"

RS232Motor::RS232Motor()
{
}
  
int RS232Motor::openRS232(const char *device, int baudrate)
{
	struct termios  options;
	int bitrate;

	bitrate = (baudrate == 9600) ? B9600 : (baudrate == 19200) ? B19200 : 0;
	if (!bitrate)
	{
		printf("ERROR : Serial %d baudrate unsupported!\n", baudrate);
		return 0;
	}

	int hd = open(device, O_RDWR | O_NOCTTY);

	if ( hd < 0 )
	{
		hd = 0;
		printf("ERROR : Serial\n");
		return 0;
	}

	tcgetattr( hd, &options);

	options.c_cflag = bitrate | CS8 | CREAD | CLOCAL;
	options.c_iflag = IGNPAR;
	options.c_oflag = 0;
	options.c_lflag = 0;

	options.c_cc[VMIN]  = 1;
	options.c_cc[VTIME] = 0;

	tcsetattr( hd, TCSANOW, &options );

	return hd;
}

int RS232Motor::readRS232(int hCOM, unsigned char *buffer, unsigned int nNumberOfBytesToRead, unsigned int *lpNumberOfBytesRead)
{
	unsigned int kk=0;
	if (!hCOM)
		return -1;

	*lpNumberOfBytesRead  = 0;
	for (kk = 0; kk < nNumberOfBytesToRead; kk++)
		*lpNumberOfBytesRead += read(hCOM, buffer + kk, 1);

	//  *lpNumberOfBytesRead = ::read(m_handle, buffer, nNumberOfBytesToRead);
	return (nNumberOfBytesToRead >= 0);
}

int RS232Motor::closeRS232( int hCOM )
{
	if ( !hCOM )
		return 0;

	close( hCOM );
	hCOM = 0;

	return 1;
}

int RS232Motor::writeRS232(int hCOM, unsigned char *buffer, unsigned int nNumberOfBytesToWrite, unsigned int *lpNumberOfBytesWritten)
{
	if (!hCOM)
		return 0;

	*lpNumberOfBytesWritten = 0;
	unsigned int kk=0;  
	for (kk = 0; kk < nNumberOfBytesToWrite; kk++)
		*lpNumberOfBytesWritten += write(hCOM, buffer + kk, 1);
	//*lpNumberOfBytesWritten = ::write(m_handle, buffer, nNumberOfBytesToWrite);
	return (*lpNumberOfBytesWritten >= 0);
}

int RS232Motor::SetRS232Motor33f_low_res(int hCOM,char speed1,char speed2) {

	unsigned int n;
	unsigned char sbuf[20];
	int ress=0;
	unsigned char tt=0;
	sbuf[0] = 255;
	sbuf[1] = 0x07;			
	int tmp1 = 8*(speed1&0x3F);
	int tmp2 = 8*(speed2&0x3F);
	if (speed2&0x80) tt=tt+32;
	if (speed2&0x40) tt=tt+16;
	sbuf[2] = (unsigned char)tmp1;
	sbuf[3] = (unsigned char)(tmp1 >> 8);
	sbuf[4] = (unsigned char)tmp2;
	sbuf[5] = (unsigned char)(tmp2 >> 8);

	sbuf[6] = (unsigned char)((speed1&0x80) + (speed1&0x40) + tt +8);

	short mycrcsend = CRC16(sbuf+1,6);

	sbuf[7] = (unsigned char)mycrcsend;
	sbuf[8] = (unsigned char)(mycrcsend >> 8);

	ress = writeRS232(hCOM, sbuf, 9,&n);
	return ress;
}

int RS232Motor::SetRS232Motor33f(int hCOM,short speed1,short speed2,unsigned char SpeedFlag) {

	unsigned int n;
	unsigned char sbuf[20];
	int ress=0;
	//unsigned char tt=0;
	sbuf[0] = 255;
	sbuf[1] = 0x07;			
	sbuf[2] = (unsigned char)speed1;
	sbuf[3] = (unsigned char)(speed1 >> 8);
	sbuf[4] = (unsigned char)speed2;
	sbuf[5] = (unsigned char)(speed2 >> 8);

	sbuf[6] = SpeedFlag;

	short mycrcsend = CRC16(sbuf+1,6);

	sbuf[7] = (unsigned char)mycrcsend;
	sbuf[8] = (unsigned char)(mycrcsend >> 8);

	ress = writeRS232(hCOM, sbuf, 9,&n);
	return ress;
}

int RS232Motor::SetRS232MotorPID33f(int hCOM,char speed1,char speed2,char pp,char ii,char dd,short maxspeed) {

	unsigned int n;

	unsigned char sbuf[20];
	int ress=0;

	sbuf[0] = 255;
	sbuf[1] = 0x09;
	sbuf[2] = speed1;
	sbuf[3] = speed2;
	sbuf[4] = pp;
	sbuf[5] = ii;
	sbuf[6] = dd;
	sbuf[7] = (char)maxspeed;
	sbuf[8] = (char)(maxspeed >> 8);

	short mycrcsend = CRC16(sbuf+1,8);
	sbuf[9] = (unsigned char)mycrcsend;
	sbuf[10] = (unsigned char)(mycrcsend >> 8);

	ress = writeRS232(hCOM, sbuf, 11, &n);
	return ress;
}

int RS232Motor::StopMotorRS23233f(int hCOM){
	SetRS232Motor33f_low_res(hCOM,0x00,0x00);
	return 1;
}

int RS232Motor::GetRS232Motor33f(int hCOM,RS232Data *dataL,RS232Data *dataR) {


	unsigned int n;
	unsigned char sbuf[30];
	int ress=0;
	int r=0;

	do { // Sync
		r = readRS232(hCOM,sbuf, 1, &n);
	} while (sbuf[0] != 255 && r);

	if (r) r = readRS232(hCOM,sbuf, 21, &n);

	short mycrcrcv = (short)((sbuf[20] << 8) + sbuf[19]);
	short mycrcsend = CRC16(sbuf,19);

	if (mycrcrcv!=mycrcsend)
	{
		do { // Sync
			r = readRS232(hCOM,sbuf, 1, &n);
		} while (sbuf[0] != 255 && r);
	}
	else 
	{
		dataL->SpeedFront=(int)((sbuf[1] << 8) + sbuf[0]);
		if (dataL->SpeedFront > 32767) dataL->SpeedFront=dataL->SpeedFront-65536;
		dataL->SpeedFront=dataL->SpeedFront;
		dataL->robot_voltage=sbuf[2];
		dataL->IR=sbuf[3];
		dataL->IR2=sbuf[4];
		dataL->odometry=((((long)sbuf[8] << 24))+(((long)sbuf[7] << 16))+(((long)sbuf[6] << 8))+((long)sbuf[5]));

		dataR->SpeedFront=(int)((sbuf[10] << 8) + sbuf[9]);
		if (dataR->SpeedFront > 32767) dataR->SpeedFront=dataR->SpeedFront-65536;
		dataR->SpeedFront=dataR->SpeedFront;
		dataR->robot_voltage=sbuf[1];
		dataR->IR=sbuf[11];
		dataR->IR2=sbuf[12];
		dataR->odometry=((((long)sbuf[16] << 24))+(((long)sbuf[15] << 16))+(((long)sbuf[14] << 8))+((long)sbuf[13]));
		dataL->robot_current=sbuf[17];
		dataR->robot_current=sbuf[17];
		dataL->robot_version=sbuf[18];
		dataR->robot_version=sbuf[18];
	}

	return ress;
}


short CRC16(unsigned char *adresse_tab , unsigned char taille_max)
{
	unsigned int Crc = 0xFFFF;
	unsigned int Polynome = 0xA001;
	unsigned int CptOctet = 0;
	unsigned int CptBit = 0;
	unsigned int Parity= 0;

	Crc = 0xFFFF;
	Polynome = 0xA001; // Polynôme = 2^15 + 2^13 + 2^0 = 0xA001.

	for ( CptOctet= 0 ; CptOctet < taille_max ; CptOctet++)
	{
		Crc ^= *( adresse_tab + CptOctet); //Ou exculsif entre octet message et CRC

		for ( CptBit = 0; CptBit <= 7 ; CptBit++) /* Mise a 0 du compteur nombre de bits */
		{
			Parity= Crc;
			Crc >>= 1; // Décalage a droite du crc
			if (Parity%2 == 1) Crc ^= Polynome; // Test si nombre impair -> Apres decalage à droite il y aura une retenue
		} // "ou exclusif" entre le CRC et le polynome generateur.
	}
	return(Crc);
}

